from PySide2 import QtCore
from PySide2.QtCore import QTimer,Qt
from PySide2.QtWidgets import QApplication, QSlider,QLCDNumber,QRadioButton,QStatusBar,QLabel
from PySide2.QtUiTools import QUiLoader
from gui.widget import model2dWidget
class BPanel:
    '''
    身体部位控制器
    '''
    def __init__(self,rootObject=None):

        self.rootObject = rootObject
        self.ui_Main = QUiLoader().load("gui/qt_ui_design/body_ctrl.ui")
        #   载入场景
        self.frontScene=model2dWidget.RobotModel2D(view='x')
        self.leftScene = model2dWidget.RobotModel2D(view='y')
        self.topScene = model2dWidget.RobotModel2D(view='z',xRange=[-0.1,0.1],yRange=[-0.1,0.1])

        self.ui_Main.fLayout.addWidget(self.frontScene)
        self.ui_Main.lLayout.addWidget(self.leftScene)
        self.ui_Main.tLayout.addWidget(self.topScene)
        self.AddTerminal()
        self.AddChain()
        #   绑定按钮
        #self.ui_Main.syncBtn.clicked.connect(self.Sync2Env)
        self.timer=QTimer()
        self.timer.timeout.connect(self.Sync)
        self.timer.start(30)

    def AddChain(self):
        self.frontScene.AddChain(self.rootObject.lChain,color=Qt.yellow)
        self.frontScene.AddChain(self.rootObject.rChain,color=Qt.blue)
        self.leftScene.AddChain(self.rootObject.lChain,color=Qt.yellow)
        self.leftScene.AddChain(self.rootObject.rChain,color=Qt.blue)
        self.topScene.AddChain(self.rootObject.lChain,color=Qt.yellow)
        self.topScene.AddChain(self.rootObject.rChain,color=Qt.blue)
    def AddTerminal(self):

        self.frontScene.AddTerminal(self.rootObject.base)
        self.frontScene.AddTerminal(self.rootObject.lFoot)
        self.frontScene.AddTerminal(self.rootObject.rFoot)
        self.leftScene.AddTerminal(self.rootObject.base)
        self.leftScene.AddTerminal(self.rootObject.lFoot)
        self.leftScene.AddTerminal(self.rootObject.rFoot)
        self.topScene.AddTerminal(self.rootObject.base)
        self.topScene.AddTerminal(self.rootObject.lFoot)
        self.topScene.AddTerminal(self.rootObject.rFoot)

    def Sync2Env(self):
        '''
        同步到实体机器人
        :return:
        '''
        #   base
        for scene in [self.frontScene,self.leftScene,self.topScene]:
            if scene.focused:
                scene.terminalList[0].UpdateTargetPos(self.rootObject.base,view=scene.view)
                scene.terminalList[1].UpdateTargetPos(self.rootObject.lFoot,view=scene.view)
                scene.terminalList[2].UpdateTargetPos(self.rootObject.rFoot,view=scene.view)
                self.rootObject.kinematic.timeRemain=1.0
                self.rootObject.kinematic.approachingTarget=True

    def Sync(self):
        self.Sync2Env()
        for scene in [self.frontScene,self.leftScene,self.topScene]:
            if not scene.focused:
                scene.terminalList[0].SetPos(self.rootObject.base,view=scene.view)
                scene.terminalList[1].SetPos(self.rootObject.lFoot,view=scene.view)
                scene.terminalList[2].SetPos(self.rootObject.rFoot,view=scene.view)

            scene.UpdateChain(0, self.rootObject.lChain)
            scene.UpdateChain(1, self.rootObject.rChain)



